#ifndef KALMAN_H
#define KALMAN_H

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <vector>

using namespace Eigen;
using namespace std;


#define dt 0.1 //sec 100ms
#define alpha 0.2

typedef Matrix<float, 2, 4> Matrix2_4f;

class Kalman
{
    public:
        Kalman();
        virtual ~Kalman();
        Vector4f process(Matrix<float, 2, 1> measure);
        Matrix<float, 2, 1> LPF(Matrix<float, 2, 1> measure);
        Matrix<float, 2, 1> AVG(Matrix<float, 2, 1> measure);
        Matrix<float, 2, 1> MOVING_AVG(Matrix<float, 2, 1> measure);
        void init();
        Matrix4f m_A,m_Q,m_P;
        Matrix2_4f m_H;
        Matrix2f m_R;
        Vector4f m_x;
        bool m_KM_initial;
        bool m_LPF_initial;
        bool m_AVG_initial;
        bool m_MOVING_AVG_initial;
        int m_total_num_samp;
        int m_moving_num_samp;
        FILE* fp_cov;

        Matrix<float, 2, 1> prevX_lpf,lpf_x;
        Matrix<float, 2, 1> prevX_avg,avg_x;
        Matrix<float, 2, 1> prevX_moving,moving_x;
    private:
        
};

#endif
